import numpy as np

def load_npy(path):
    data = np.load(path, allow_pickle=True)
    # 如果 data 是一个列表或元组，可能需要进一步处理
    if isinstance(data, (np.ndarray, list, tuple)):
        print(data[0])
        return data[0][0]
        # for array in data:
        #     print(array)
    else:
        print(data)


def  read_accleration(path):
    # 打开文件
    with open(path , 'r') as file:
        lines = file.readlines()

    # 初始化三个空列表来保存 x, y 和 z 值
    x_values = []
    y_values = []
    z_values = []

    # 逐行读取并解析数据
    for line in lines:
        # 删除行首尾的空白字符，如果处理后的行不为空字符串则继续
        stripped_line = line.strip()
        if stripped_line:  # 如果行非空
            # 使用空格分隔每一行，并将结果转换为浮点数
            x, y, z = map(float, stripped_line.split())
            x_values.append(x)
            y_values.append(y)
            z_values.append(z)

    return x_values,y_values,z_values

def average(data_list):
    return sum(data_list) / len(data_list)

if __name__=="__main__":
    # name="root_list.npy"

    # real_obs_path="/home/xjz/catkin_ws/src/my_get_model_state_pkg/scripts/obs_action_data/"+name
    # gazebo_obs_path="/home/xjz/catkin_ws/src/my_get_model_state_pkg/scripts/gazebo_numpy/"+name
    # real_obs_data=load_npy(real_obs_path)
    # gazebo_obs__data=load_npy(gazebo_obs_path)
    # minues=real_obs_data-gazebo_obs__data


    path="/home/xjz/catkin_ws/src/my_get_model_state_pkg/scripts/obs_action_data/accleration.txt"
    x_values,y_values,z_values=read_accleration(path)
        # 输出结果，检查是否正确
    print("X values:",average(x_values))
    print("Y values:", average(y_values))
    print("Z values:", average(z_values))


    pass